package cn.com.fhc.ros

import cn.com.fhc.ros.services.ServiceRequest
import cn.com.fhc.ros.services.ServiceResponse
import java.util.concurrent.locks.Condition
import java.util.concurrent.locks.ReentrantLock
import javax.json.Json
import javax.json.JsonObject
import kotlin.concurrent.withLock

class Service {


    private  var ros: Ros
        get() = field
    private  var name: String
        get() = field
    private var type: String? = null
        get() = field
    private var isAdvertised = false
        get() = field

    /**
     * Create a ROS service with the given information.
     *
     * @param ros
     * A handle to the ROS connection.
     * @param name
     * The name of the service (e.g., "/add_two_ints").
     * @param type
     * The service type (e.g., "rospy_tutorials/AddTwoInts").
     */
    constructor(ros: Ros, name: String, type: String?) {
        this.ros = ros
        this.name = name
        this.type = type
        isAdvertised = false
    }



    /**
     * Call this service. The callback function will be called with the
     * associated service response.
     *
     * @param request
     * The service request to send.
     * @param cb
     * The callback used when the associated response comes back.
     */
    private fun callService(request: ServiceRequest, cb: ServiceResponseCallback) {
        // construct the unique ID
        val callServceId = ("call_service:" + this.name + ":"
                + this.ros.nextId())

        // register the callback function
        this.ros.registerServiceCallback(callServceId, cb)

        // build and send the rosbridge call
        val call: JsonObject = Json.createObjectBuilder()
            .add(FIELD_OP, OP_CODE_CALL_SERVICE)
            .add(FIELD_ID, callServceId)
            .add(FIELD_TYPE, this.type)
            .add(FIELD_SERVICE, this.name)
            .add(FIELD_ARGS, request.toJsonObject()).build()
        this.ros.send(call)
    }

    /**
     * Send a service response.
     *
     * @param response
     * The service response to send.
     * @param id
     * The ID of the response (matching that of the service call).
     */
    fun sendResponse(response: ServiceResponse, id: String?) {
        // build and send the rosbridge call
        val call: JsonObject = Json.createObjectBuilder()
            .add(FIELD_OP, OP_CODE_SERVICE_RESPONSE)
            .add(FIELD_ID, id)
            .add(FIELD_SERVICE, this.name)
            .add(FIELD_VALUES, response.toJsonObject())
            .add(FIELD_RESULT, response.result).build()
        this.ros.send(call)
    }

    /**
     * Registers as service advertiser.
     */
    fun advertiseService(cb: ServiceRequestCallback) {
        // register the callback
        this.ros.registerCallServiceCallback(this.name, cb)

        // build and send the rosbridge call
        val call: JsonObject = Json.createObjectBuilder()
            .add(FIELD_OP, OP_CODE_ADVERTISE_SERVICE)
            .add(FIELD_TYPE, this.type)
            .add(FIELD_SERVICE, this.name).build()
        this.ros.send(call)

        // set the flag indicating we are registered
        this.isAdvertised = true
    }

    /**
     * Unregisters as service advertiser.
     */
    fun unadvertiseService() {
        this.ros.deregisterCallServiceCallback(this.name)

        // build and send the rosbridge call
        val call: JsonObject = Json.createObjectBuilder()
            .add(FIELD_OP, OP_CODE_UNADVERTISE_SERVICE)
            .add(FIELD_SERVICE, this.name).build()
        this.ros.send(call)

        // set the flag indicating we are registered
        this.isAdvertised = false
    }

    private val lock = ReentrantLock()
    private val condition = lock.newCondition()

    fun callServiceAndWait(request: ServiceRequest): ServiceResponse? {

        // private inner class to use as a callback
        val cb = BlockingCallback(this,condition)
        // use the asynchronous version and block on the result
        callService(request, cb)

        lock.withLock {           // like synchronized(lock)
            // wait for a response
            while (cb.getResponse() == null) {
                try {
                    condition.await()
                } catch (e: InterruptedException) {
                    // continue on
                }
            }
        }
        return cb.getResponse()
    }



    private class BlockingCallback : ServiceResponseCallback {

        private var response: ServiceResponse? = null
        private  var service: Service
        private  var condition: Condition

        constructor(service: Service, condition: Condition) {
            this.response = null
            this.service = service
            this.condition = condition
        }
        /**
         * Get the response stored in this callback, if one exists. Otherwise,
         * null is returned.
         *
         * @return The resulting service response from ROS, or null if one does
         * not exist yet.
         */
        fun getResponse(): ServiceResponse? {
            return this.response
        }
        override fun handleServiceResponse(response: ServiceResponse) {
            this.response = response
            condition.signalAll()
        }
    }

}
